#ifndef __SHOOTER_H_
#define __SHOOTER_H_
#include "main.h"
#include "can.h"
#include "shooter_clutch.h"
#include "laser.h"

typedef enum _shooter_mode_t
{
    SHOOTER_MODE_DYNAMIC,
    SHOOTER_MODE_FIXED,
} shooter_mode_t;

/**
 * 5个字母的状态: 等待外部事件
 * 4个字母的状态: 等待内部事件
 * 3个字母的状态: 持续控制到目标位置
 * :)
 **/
typedef enum _shooter_state_t
{
    SHOOTER_STATE_START,  // 流程开始, 等待上箭机构完成取箭+流程开始标志
    SHOOTER_STATE_ROTATE, // 转移到上箭位, 周期性检查是否到达上箭位: 到达则进入上箭状态, 未到达则发控制指令
    SHOOTER_STATE_LOAD,   // 上箭, 等待上箭完毕
    SHOOTER_STATE_READY,  // 上箭完毕, 等待瞄准标志
    SHOOTER_STATE_AIMI,   // 瞄准+预调整, 等待瞄准完成 (没有云台的射箭机构没有该状态)
    SHOOTER_STATE_ADJUST, // 根据瞄准得到的参数进行调整, 周期性检查是否到达射箭位: 到达则进入射箭状态, 未到达则发控制指令
    SHOOTER_STATE_SHOOT,  // 射箭, 等待射箭标志, 等待完成则发射
    SHOOTER_STATE_MAX,
} shooter_state_t;

/* 射箭机构的状态: 拉力, 俯仰角, 偏航角 */
typedef struct _shooter_status_t
{
    int force;
    int pitch;
    int yaw;
} shooter_status_t;

typedef struct _shooter_pid_t
{
    float Kp;
    float Kd;
    float KI_P;
    float KI_N;
    float i;
    float last_err;
    float i_max_p;
    float i_max_n;
    float i_range_p;
    float i_range_n;
} shooter_pid_t;

typedef struct _shooter_t
{
    CAN_HandleTypeDef *hcan;
    // int vesc_id;
    int dji_board_id;
    int dji_fmotor_id;    // 拉力电机
    int dji_pmotor_id;    // 俯仰电机
    int dji_ymotor_id;    // 云台电机
    laser_handle_t laser; // 激光
    uint8_t init;
    int point;
    shooter_mode_t mode;
    shooter_state_t state;
    int now_force_pos;
    int now_round_cnt;
    shooter_status_t now_stat;
    shooter_status_t tar_stat;
    shooter_pid_t force_pid;
    shooter_clutch_t clutch;
    osEventFlagsId_t events;
    osMessageQueueId_t queue; // 上层模块向射箭机构发送命令
    osMutexId_t mutex;        // 针对now_stat中的force变量的互斥锁
    osThreadId_t task;
    /* dynamic */
    int last_force_pos;
} shooter_t;
typedef struct _shooter_t *shooter_handle_t;

typedef struct _shooter_init_t
{
    void *clutch_port;
    uint16_t clutch_pin;
    CAN_HandleTypeDef *motor_hcan;
    // int vesc_id;
    int dji_board_id;
    int dji_fmotor_id;
    int dji_pmotor_id;
    int dji_ymotor_id;
    laser_handle_t laser;
} shooter_init_t;

void shooter_init(shooter_handle_t sht, const shooter_init_t *init);

int shooter_test_start(shooter_handle_t sht);
int shooter_test_shoot(shooter_handle_t sht);

int shooter_set_param(shooter_handle_t sht, int force, int pitch);
int shooter_load_ready(shooter_handle_t sht);
int shooter_start_aim(shooter_handle_t sht);

void shooter_force_update(shooter_handle_t sht, int force_pos);
void shooter_pitch_update(shooter_handle_t sht, int pitch_pos);

void shooter_pitch_ready(shooter_handle_t sht);
void shooter_pitch_homing_ready(shooter_handle_t sht);
void shooter_yaw_ready(shooter_handle_t sht);
void shooter_yaw_homing_ready(shooter_handle_t sht);
void shooter_yaw_arrive(shooter_handle_t sht, float dist);

void shooter_pause(shooter_handle_t sht);
void shooter_restart(shooter_handle_t sht);

void shooter_set_point(shooter_handle_t sht, int point);
void shooter_set_mode(shooter_handle_t sht, shooter_mode_t mode);
void shooter_read_yaw(shooter_handle_t sht);

extern shooter_handle_t shooter1;

#endif /* !__SHOOTER_H */